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Figure 13 



Receive the angular 
increments from the 

Torus FOG, k6 z . 



Remove the FOG drift. 
&6 2 = &9 2 -d z Ar 



Receive the angular 
increments from the 

Normal FOG, A#. 



y ' 



Remove the FOG drift. 
&9 y = &9 y -d y A; 



Receive the velocity increments 
from the accelerometers, 



Remove the accelerometer bias terms. 
Av x = AVj -b x A^Av^ = Av^ -b y A/, 
Av r = Av ? - b : A/ 



Determine the effect of Earth rotation 
and the £ -frame change of orientation. 



M + h 

N + h 
V e (t k )tan<p 
N + h 



COS $7 

+ co e ship 



0*=a>*Af = (^ dO y dd,f 



J 



Update the quaternion and get R l b . 
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Determine the orientation of the bottom hole 
assembly as follows. 

Pitch (inclination) : 9 - arcsin(£ 3 2) 
Roll : <t> = arctan(- R 3 JR 23 ) 
Azimuth (toolface) : ip = arctan(- R\ 2 I ^22) 



Evaluate the pitch angle (inclination). 


9 = arctan 


r ^ 




W + Av, 2 



Determine the velocity changes of the bottom, 
hole assembly. 



Update the velocity components of the bottom- 
hole assembly. 

V l {t M ) = V l {t k ) + H&V l (t k ) + ±V l (t M )) 



Update the position components of the bottom 
hole assembly. 

*<<„,)- *(<») + j(c"('„„) + r(<„))a< 

2 M + /z 
2 {N + h)CQS<p 



Figure 14 



Receive the angular 
increments from the 

Torus FOG, AO z . 



1 


r 


Remove the 
A9 Z = A6 


J FOG drift. 



Receive the velocity 
increments from the 
accelerometers, 



Determine the effect of Earth rotation 
and the i -frame change of orientation. 



4 



N + h 
V e (t k )ten<p 



+ o cos<p 
+ co e sm<p 



N + h 

eb=m> t to=(d9 x d9 y dej 

8 b ib=9i-e>t = {5e x S9 y so,) 
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Update the quaternion and get R l b . 
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Determine the orientation of the bottom 
hole assembly as follows. 

Pitch (inclination) : 0 = arcsin(£ 32 ) 
Roll : <f> = arctan(- R 3l /R 33 ) 
Azimuth (toolface) : iff = arctan(- R l2 /R 2 2) 



Remove the accelerometer bias terms. 
A v x = Av x - b x A/, Av y = Av, - b y A/, 
Av f = Av. -b z At 



Evaluate the pitch angle (inclination) 
and the roll angle. 



r 



0 = arctan 



Avi 



Av* + Av} 
v. ' * 'J 



-Av, 



<j> - arctan 



Determine the velocity changes of the bottom 
hole assembly. 



Update the velocity components of the bottom 
hole assembly. 

vl i^) = V\t k )MAV\t k ) + AV\t M )) 



Update the position components of the bottom 
hole assembly. 

2 M + h 

2 [N + h)cos<p 



Figure 14A 



Receive the angular 
increments from the 

Normal FOG, A9 Z . 



Receive the angular 
increments from the 
Toms FOG, A0 V . 



Remove the FOG drift. 
A9 Z = A9 Z ~d z At 



Receive the velocity increments 
from the accelerometers, 



Remove the FOG drift. 
A9 y =A0 y -d At 



Determine the effect of Earth rotation 
and the I -frame change of orientation. 

N + h 
V e (t k )tan<p 



+ 0) co$<p 



N + h 

9? £ =o> b u At = (d9 x d9 y d&J 
e^{A9 x A9 y A9 z f 
9ib=4-e«={se x 8Q y S9 S ) 



Update the quaternion and get R l b . 
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Determine the orientation of the bottom hole 
assembly as follows. 

Pitch :6> = arcsin(£ 32 ) 

inclination = 7r/2-9 

Roll (toolface) : (j> = arctan(- R 3 , /R 33 ) 

Azimuth : y/ = arctan(- R n /R 2 2) 



Remove the accelerometer bias terms. 
Av x = Av x - b x At t Av y = Av, - b y At, 
Av : =Av : -b z At 



Evaluate the pitch angle (inclination). 



9 = arctan 



Avf 



' Av^ + Av* 



Determine the velocity changes of the bottom 
hole assembly. 

^'(^ + l) = ^AvV A+l )-(2Q / t + 0^(/ A )A/ + ^'A/ 



Update the velocity components of the bottom 
hole assembly. 

^(^.i) = ^(^)^(A^(r,) + A^ +l) ) 



Update the position components of the bottom 
hole assembly. 

l_ 

' 2 



At 



M + h : 

(;V + /z)cos^ 



Figure 15 




Figure 1 6 



Receive the Torus FOG angular 
increments and remove the drift. 



Receive the acceierometer velocity 
increments and remove the bia 
terms. 

Av x = - b x Af, AVy = Av^, - b y A/, 
Av x = Av 2 - b s At 



Determine the 
angular velocity 
measurement 



Ar 



Determine the corresponding 
specific force measurement 

» h 



A/ 



A/ ,/ * = A/ 



Compute the cos^ 
term 



cosy/ = 



a; cos<? 



* sin # tan 



cos 5 



Compute the error 


8 cosy/ term 
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Corrected azimuth 
angle 



Determine the pitch 
and the roll angles 
f i ; — \ 



9 ~ arctan 



/. 2 



\ft+fl 



$ = arctan 



A) 



Determine the error in 
the pitch and the roll 



69 = - 



ycosB 



6<j> = 



V^/x + fl? - ffj? tan 2 (9 



y cos 0 



Corrected pitch and roll 
angles 



Figure 1 7 



FOGs and accelerometer 
Measurements. 
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Navigation 
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Figure 18 



